#include <ros/ros.h>
#include "mcu_correspond/mcu_data_subscriber.h"
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <cmath>
#include <locale>
#include <algorithm>
#include <limits>
#include "my_pcl/my_pcl.h"

// 全局变量存储圆环位置
double torus1_x = 0.0;         // 圆环1的x坐标
double torus1_y = 0.0;         // 圆环1的y坐标
double torus1_z = 0.0;         // 圆环1的z坐标
bool torus1_found = false;     // 圆环1是否找到
bool torus1_stored = false;    // 圆环1位置是否已存储
bool torus1_published = false; // 圆环1位置是否已发布

double traj1x = 0.0, traj1y = 0.0, traj1z = 1.5; // 轨迹1的目标位置
double traj2x = 0.0, traj2y = 0.0, traj2z = 1.5; // 轨迹2的目标位置
double traj3x = 0.0, traj3y = 0.0, traj3z = 1.5; // 轨迹3的目标位置

void publishTarjData(McuDataSubscriber &mcu_data_)
{
    // 打印当前轨迹点的值
    ROS_INFO("=== 当前轨迹点数据 ===");
    ROS_INFO("轨迹点1 (接近点): x=%.3f, y=%.3f, z=%.3f", traj1x, traj1y, traj1z);
    ROS_INFO("轨迹点2 (中心点): x=%.3f, y=%.3f, z=%.3f", traj2x, traj2y, traj2z);
    ROS_INFO("轨迹点3 (离开点): x=%.3f, y=%.3f, z=%.3f", traj3x, traj3y, traj3z);

    // 发布轨迹点数据
    if (traj1x == mcu_data_.getDoubleValue("traj1_x") &&
        traj1y == mcu_data_.getDoubleValue("traj1_y") &&
        traj2x == mcu_data_.getDoubleValue("traj2_x") &&
        traj2y == mcu_data_.getDoubleValue("traj2_y") &&
        traj3x == mcu_data_.getDoubleValue("traj3_x") &&
        traj3y == mcu_data_.getDoubleValue("traj3_y"))
    {
        ROS_INFO("轨迹数据未变化，跳过发布");
        return; // 如果数据未变化，则不发布
    }

    mcu_data_.publishDoubleData("traj1_x", traj1x);
    mcu_data_.publishDoubleData("traj1_y", traj1y);

    mcu_data_.publishDoubleData("traj2_x", traj2x);
    mcu_data_.publishDoubleData("traj2_y", traj2y);

    mcu_data_.publishDoubleData("traj3_x", traj3x);
    mcu_data_.publishDoubleData("traj3_y", traj3y);

    ROS_INFO("✓ 轨迹数据已发布");
}

class TorusDetector
{
private:
    ros::NodeHandle nh_;
    McuDataSubscriber mcu_data_;
    ros::Subscriber pointcloud_sub_;
    ros::Timer trajectory_timer_; // 轨迹数据发布定时器
    ICP_Matching icp_matcher_;

public:
    TorusDetector(ros::NodeHandle &nh) : nh_(nh), mcu_data_(nh)
    {
        const std::string template_path = "/home/firefly/ws_vision/src/flypcl/pcd/";
        const double fitness_threshold = 0.001;
        icp_matcher_ = ICP_Matching(template_path, fitness_threshold);
        std::vector<std::string> template_files = {"circle.pcd"};
        icp_matcher_.setTemplateFiles(template_files);
        icp_matcher_.setICPParameters(200, 1e-5, 1e-5, 0.5);
        icp_matcher_.setClusterFilterParameters(50);
        pointcloud_sub_ = nh_.subscribe("/livox/map", 1, &TorusDetector::pointCloudCallback, this);
        trajectory_timer_ = nh_.createTimer(ros::Duration(2.0),
                                            &TorusDetector::trajectoryTimerCallback, this);
        
        ROS_INFO("Torus Detection System initialized with ICP_Matching library");
        ROS_INFO("Template path: %s", template_path.c_str());
        ROS_INFO("Publishing topics: /clusters_cloud, /detected_objects, /object_trajectory");
        ROS_INFO("轨迹数据发布定时器已启动 (每2秒执行一次)");
    }

    void detectAndUpdateTorus(const sensor_msgs::PointCloud2ConstPtr &cloud_msg)
    {
        try
        {
            if (torus1_stored)
            {
                return;
            }
            std::string currentStepName = mcu_data_.getStringValue("state");
            if (currentStepName != "wait_for_traj")
            {
                ROS_INFO_THROTTLE(1.0, "当前状态: %s，等待主控节点发布", currentStepName.c_str());
                return;
            }
            pcl::PointCloud<PointType>::Ptr cloud(new pcl::PointCloud<PointType>);
            pcl::fromROSMsg(*cloud_msg, *cloud);
            auto clusters = processCloudsForStickDetection(cloud, "/clusters_cloud", cloud_msg->header.frame_id);
            std::vector<ICPMatchResult> torus_matches = icp_matcher_.findMatches(clusters, true);
            icp_matcher_.publishVisualization(torus_matches, cloud_msg->header,
                                              "/detected_torus", "/torus_trajectory");
            torus1_found = false;
            if (!torus_matches.empty())
            {
                // 按y坐标排序（y大的排在前面）
                std::sort(torus_matches.begin(), torus_matches.end(),
                          [](const ICPMatchResult &a, const ICPMatchResult &b)
                          {
                              return a.y_position > b.y_position;
                          });
                if (torus_matches.size() >= 1 && !torus1_stored)
                {
                    torus1_x = torus_matches[0].x_position;
                    torus1_y = torus_matches[0].y_position;
                    torus1_z = torus_matches[0].z_position;
                    torus1_found = true;
                    torus1_stored = true;
                    TrajectoryPoints trajectory = icp_matcher_.generateTrajectory(torus_matches[0], 0.5);
                    traj1x = trajectory.approach_point.x;
                    traj1y = trajectory.approach_point.y;
                    traj2x = trajectory.center_point.x;
                    traj2y = trajectory.center_point.y;
                    traj3x = trajectory.exit_point.x;
                    traj3y = trajectory.exit_point.y;
                    ROS_INFO("圆环1位置已锁定: x=%.3f, y=%.3f, z=%.3f, 匹配度=%.6f",
                             torus1_x, torus1_y, torus1_z, torus_matches[0].fitness_score);
                    ROS_INFO("生成穿越轨迹:");
                    ROS_INFO("  接近点: (%.3f, %.3f, %.3f)", traj1x, traj1y, traj1z);
                    ROS_INFO("  中心点: (%.3f, %.3f, %.3f)", traj2x, traj2y, traj2z);
                    ROS_INFO("  离开点: (%.3f, %.3f, %.3f)", traj3x, traj3y, traj3z);
                    ROS_INFO("  法向量: (%.3f, %.3f, %.3f)",
                             torus_matches[0].normal_vector[0],
                             torus_matches[0].normal_vector[1],
                             torus_matches[0].normal_vector[2]);
                    ROS_INFO("  半径估算: %.3f", torus_matches[0].radius);
                    ROS_INFO("🔒 圆环检测已完成，停止后续检测以节省计算资源");
                }
            }
            else
            {
                ROS_WARN("未检测到符合条件的圆环，继续搜索...");
            }
        }
        catch (const std::exception &e)
        {
            ROS_ERROR("圆环检测错误: %s", e.what());
        }
    }

    void trajectoryTimerCallback(const ros::TimerEvent &)
    {
        // 只有在圆环锁定后才定期发布轨迹数据
        if (torus1_stored)
        {
            publishTarjData(mcu_data_);
        }
        else
        {
            static int count = 0;
            if (++count % 5 == 0)
            {
                std::string currentStepName = mcu_data_.getStringValue("state");
                ROS_INFO("当前状态: %s", currentStepName.c_str());
                ROS_INFO("🔍 正在搜索圆环，轨迹数据暂未可用...");
            }
        }
    }

    void pointCloudCallback(const sensor_msgs::PointCloud2ConstPtr &cloud_msg)
    {
        detectAndUpdateTorus(cloud_msg);
    }
};

int main(int argc, char **argv)
{
    std::setlocale(LC_ALL, "zh_CN.UTF-8");

    ros::init(argc, argv, "s2022circle");
    ros::NodeHandle nh;

    // Create torus detector with ICP_Matching library
    TorusDetector detector(nh);

    ROS_INFO("Torus Detection System started - Using ICP_Matching library");
    ROS_INFO("Torus assignment: Torus1 (higher y-value)");
    ROS_INFO("ICP template matching enabled for torus detection");

    ros::spin();
    return 0;
}
